package robotics;

import InterpretadorV2.InterpretadorV2;

public class GeometryRobot extends InterpretadorV2 {

    private boolean debug = false;
    private final double radius = 28; // mm
    private final double dbw = 95; // mm
    private final int CALCULATE = 1;
    private final int READ = 2;
    private final int VERIFY = 3;
    private final int STRAIGHT_OFFSET = 20;// Minimum needed distance to move and error offset
    private double id = 0;
    double factor; // factor between left and right wheel in curve move
    double radiusMax;
    double radiusMin;
    double speedMin;
    double speedMax;
    double distanceMax;
    double distanceMin;
    double distance;
    private double pd = 0;
    private double degrees;
    final int speedRobot = 70;
    double offsetr = 3;
    double offsetl = 0;

    /**
     * must be at least 20 mm
     *
     * @param distance in mm
     */
    void straight(double distance) {
        System.out.println("!!!");

        if (Math.abs(distance) < STRAIGHT_OFFSET) {
            throw new IllegalArgumentException("distance must be > " + STRAIGHT_OFFSET);
        }
        distance -= (STRAIGHT_OFFSET * Math.signum(distance));

        int state = CALCULATE;
        while (true) {
            switch (state) {
                case CALCULATE:
                    degrees = Calculate(distance);
                    int l = Integer.parseInt(ControlarRobot.jLeftOffset.getText());
                    int r = Integer.parseInt(ControlarRobot.jRightOffset.getText());
                    if (distance < 0) {
//                        OnFwd(OUT_BC, -speedRobot);
                        OnFwd(OUT_B, speedRobot - (int) l,
                                OUT_C, speedRobot - (int) r);
                    } else {
                        OnFwd(OUT_B, -(speedRobot - (int) l),
                                OUT_C, -(speedRobot - (int) r));
                    }
                    state = READ;
                    break;
                case READ:
                    id = MotorRotationCount(OUT_B);
                    System.out.println("id " + id);
                    state = VERIFY;
                    break;
                case VERIFY:
                    pd = MotorRotationCount(OUT_B);

                    if ((pd - id) > degrees) {
                        System.out.println("degrees = " + degrees);
                        System.out.println("(pd - id) = " + (pd - id));
                        return;
                    }
                    break;
            }
        }
    }

    private double Calculate(double distance) {
        return ((distance / radius) * (180 / Math.PI));
    }

    public void curveLeft(double radius, double angle) {
        int state = CALCULATE;
        int pd[] = {0, 0};
        int id[] = {0, 0};
        while (true) {
            switch (state) {
                case CALCULATE:
                    if (CalculateCurveMove(radius, angle)) {
                        int l = Integer.parseInt(ControlarRobot.jLeftOffset.getText());
                        int r = Integer.parseInt(ControlarRobot.jRightOffset.getText());
                        OnFwd(OUT_C, (int) (speedMin - l), OUT_B, (int) (speedMax - r));
                        state = READ;
                        break;
                    } else {
                        System.out.println("Smth is wrong, robot will not move");
                        return;
                    }
                case READ:
                    id = MotorRotationCount(OUT_B, OUT_C);
                    state = VERIFY;
                    break;
                case VERIFY:
                    pd = MotorRotationCount(OUT_B, OUT_C);
                    if (((pd[0] + pd[1]) / 2 - (id[0] + id[1]) / 2) > distance) {
                        return;
                    }
                    break;
            }
        }
    }

    public void curveRight(double radius, double angle) {
        int state = CALCULATE;
        int pd[] = {0, 0};
        int id[] = {0, 0};
        while (true) {
            switch (state) {
                case CALCULATE:
                    if (CalculateCurveMove(radius, angle)) {
                        int l = Integer.parseInt(ControlarRobot.jLeftOffset.getText());
                        int r = Integer.parseInt(ControlarRobot.jRightOffset.getText());
                        OnFwd(OUT_C, (int) (speedMax - l), OUT_B, (int) (speedMin - r));
                        state = READ;
                        break;
                    } else {
                        System.out.println("Smth is wrong, robot will not move");
                        return;
                    }
                case READ:
                    id = MotorRotationCount(OUT_B, OUT_C);
                    state = VERIFY;
                    break;
                case VERIFY:
                    pd = MotorRotationCount(OUT_B, OUT_C);
                    if (((pd[0] + pd[1]) / 2 - (id[0] + id[1]) / 2) > distance) {
                        return;
                    }
                    break;
            }
        }
    }
    
//    public void curveRight(double radius, double angle) {
//        int state = CALCULATE;
//        while (true) {
//            switch (state) {
//                case CALCULATE:
//                    //TODO add different motors count as it is in curveLeft
//                    CalculateCurveMove(radius, angle);
//                    OnFwd(OUT_C, (int) speedMax, OUT_B, (int) speedMin);
//                    state = READ;
//                    break;
//                case READ:
//                    id = MotorRotationCount(OUT_C);
//                    state = VERIFY;
//                    break;
//                case VERIFY:
//                    pd = MotorRotationCount(OUT_C);
//                    if ((pd - id) > distanceMax) {
//                        return;
//                    }
//                    break;
//            }
//        }
//    }

    private boolean CalculateCurveMove(double radius, double angle) {
        if (radius <= dbw / 2 || angle < 25) {
            System.out.println("Radius or angle is too small");
            return false;
        } else {
            angle = Math.toRadians(angle);
            distance = radius * angle; //Perimeter of arc of the robot circle
            distanceMin = (radius - dbw / 2) * angle; //Perimeter of arc of the left wheel circle
            distanceMax = (radius + dbw / 2) * angle; //Perimeter of arc of the right wheel circle
            factor = distanceMax / distanceMin;
            speedMax = (2 * factor) / (factor + 1) * speedRobot;
            speedMin = 2 / (factor + 1) * speedRobot;
            speedMax = Math.floor(speedMax);
            speedMin = Math.floor(speedMin);
            factor = speedMax / speedMin;//recalculating factor after rounding speeds
            return true;
        }
    }

    /**
     * *
     * Calculate first trajectory of the robot after calculating executes the
     * following commands to move robot: curveLeft(), straight(), curveLeft()
     *
     * @param x x coordinate
     * @param y y coordinate
     * @param f the angle in radiance
     * @return
     */
    private double[] firstTrajectory(double x, double y, double f) {
        // a, b, c coefficients of square equation
        double distance;//straight distance
        double a;
        double b;
        double c;
        double r1, r2; //Solutions of quadratic equation
        double d; //Discriminant of quadratic equation
        double angle;//angle of curveLeft/curveRight move
        a = 2 + 2 * Math.cos(f);
        b = 2 * y * (1 - Math.cos(f)) + 2 * x * Math.sin(f);
        c = -(Math.pow(x, 2) + Math.pow(y, 2));
        d = Math.pow(b, 2) - 4 * c * a;
        if (d < 0) {
            System.out.println("Discriminant is negative = " + d);
            return new double[]{};
        } else {
            r1 = (-b + Math.sqrt(Math.pow(b, 2) - 4 * c * a)) / (2 * a);//XXX:ZHOPA!!!
            r2 = (-b - Math.sqrt(Math.pow(b, 2) - 4 * c * a)) / (2 * a);//XXX:ZHOPA!!!
            angle = Math.asin((x - r1 * Math.sin(f)) / 2 * r1);
            if (CalculateCurveMove(r1, angle)) {
                r1 = (dbw / 2) * ((factor + 1) / (factor - 1));// recalc radius from (1 trajectory pdf)
                distance = Math.sqrt(Math.pow((-x + r1 * Math.sin(f)), 2) //from (1 trajectory pdf)
                        + Math.pow(r1 - (y + r1 * Math.cos(f)), 2));
                angle = Math.acos((x - r1 * Math.sin(f)) / distance);//angle of the first curve
                //(f-angle) angle of the second curve
                return new double[]{r1, angle, distance, f - angle};
            } else {
                return new double[]{};
            }
        }

//        angle = Math.asin((x - r * Math.sin(f)) / 2 * r);
    }

    void stop(Boolean bool) {
        Float(OUT_BC);
    }

    public void setDebug(Boolean debug) {
        this.debug = debug;
    }

    public void log(String str) {
        if (debug) {
            System.out.println(str);
        }
    }

    public static void main(String[] args) {
        GeometryRobot rob = new GeometryRobot();
        if (rob.OpenNXT("Guia4")) {
            System.out.println("Going to start");
            // rob.testOffset();
            rob.straight(1000);
//            rob.CurveLeft(190, 90);
//            rob.CurveRight(190, 90);
//            rob.straight(-200);
//            rob.curveLeft(3000, 90);
            System.out.println("Going to stop");
            rob.stop(true);
            rob.EsperarSegundos(1);
            rob.CloseNXT();
        }
        System.exit(0);
    }
}
